package dipl.algorithm.math.curve;

import dipl.algorithm.math.geometry.op.Point2dSetOps;
import dipl.algorithm.math.utility.ApfloatUtils;
import dipl.algorithm.math.primitive.Interval1d;
import dipl.algorithm.math.primitive.Point2d;
import dipl.algorithm.math.utility.FPMath;
import dipl.algorithm.math.primitive.Side;
import dipl.algorithm.math.primitive.Tuple2;
import dipl.algorithm.math.primitive.Vector2d;
import dipl.algorithm.math.primitive.op.Vector2dOps;
import dipl.algorithm.exception.ArgumentException;
import dipl.algorithm.exception.ArgumentNullException;
import dipl.algorithm.math.geometry.BoundingWedge2d;
import dipl.algorithm.math.primitive.op.ComplexOps;
import java.util.LinkedList;
import org.apfloat.Apfloat;

/**
 * Represents a bezier curve of arbitrary degree in the plane
 */
public class BezierCurve {

	//
	// CONSTRUCTOR METHODS
	//

	/**
	 * Constructs curve of degree n
	 * @param degree
	 * @throws ArgumentException degree is smaller than zero
	 */
	public BezierCurve( int degree ) throws ArgumentException {
		if( degree < 0 )
			throw new ArgumentException( "Degree must be greater or equal zero!" );

		this.degree = degree;
		this.coeff = null;
		this.domain = Interval1d.UnitInterval();

		this.controls = new Point2d[degree+1];
		for( int i = 0; i <= degree; i++ )
			this.controls[i] = new Point2d();
	}

	/**
	 * Constructs an exact copy of given curve c
	 * @param c
	 * @throws ArgumentNullException c is null
	 */
	public BezierCurve( BezierCurve c ) throws ArgumentNullException {
		if( c == null )
			throw new ArgumentNullException();

		this.degree = c.degree;
		this.domain = new Interval1d( c.domain );
		this.coeff = null;
		if( c.coeff != null ) {
			this.coeff = new Apfloat[c.coeff.length];
			for( int i = 0; i < this.coeff.length; i++ )
				this.coeff[i] = ApfloatUtils.CopyOf( c.coeff[i] );
		}
		this.controls = new Point2d[degree+1];
		for( int i = 0; i <= degree; i++ )
			controls[i] = new Point2d( c.controls[i] );
	}

	/**
	 * Constructs curve from given control points
	 * @param controls
	 * @throws ArgumentNullException control point array is null
	 * @throws ArgumentException control points array is empty
	 */
	public BezierCurve( Point2d[] controls ) throws ArgumentNullException, ArgumentException {
		if( controls == null )
			throw new ArgumentNullException( "Control points array is null!" );
		if( controls.length < 1 )
			throw new ArgumentException( "Control points array is empty!" );

		degree = controls.length-1;
		coeff = null;
		domain = Interval1d.UnitInterval();

		this.controls = controls;
	}

	//
	// PROPERTIES
	//

	/**
	 * @return control points of curve
	 */
	public Point2d[] Controls() {
		return controls; 
	}

	/**
	 * Sets control polygon of curve
	 * @param controls
	 * @throws ArgumentException
	 * @throws ArgumentNullException
	 */
	public void SetControls( Point2d[] controls ) throws ArgumentException, ArgumentNullException {
		if( controls == null )
			throw new ArgumentNullException();
		if( controls.length != degree+1 )
			throw new ArgumentException( "Number of control points must equal degree+1!" );
		this.controls = controls;
	}

	/**
	 * @return degree of curve's polynom
	 */
	public int Degree() {
		return degree;
	}

	/**
	 * @return parameter domain
	 */
	public Interval1d Domain() {
		return domain;
	}

	/**
	 * Sets parameter domain
	 * @param domain
	 */
	public void SetDomain( Interval1d domain ) {
		this.domain = domain;
	}

	//
	// PUBLIC METHODS
	//

	/**
	 * Calculates point on bezier-curve at parameter t
	 * @param fpMath
	 * @param t
	 * @return
	 */
	public Point2d Calculate( FPMath fpMath, Apfloat t ) {
		return CalculatePointDeCasteljau( fpMath, t );
	}

	/**
	 * Calculates n points at parameter values {a,a+e,a+2e,...,b} where e=(b-a)/n.
	 * @param fpMath fixed precision arithmetic
	 * @param points
	 */
	public void CalculatePoints( FPMath fpMath, Point2d[] points ) throws ArgumentException {
		if( points == null || points.length == 0 )
			return;

		int n = points.length-1; // index of last point to calculate
		Apfloat t = fpMath._0; // current local parameter
		Apfloat deltaT = fpMath.m.divide( fpMath._1, fpMath.ValueOf( (double)n ) ); // parameter increment

		Point2d point = new Point2d(); // current point calculated

		for( int k = 0; k <= n; k++ ) {
			CalculatePointDeCasteljau( fpMath, t, point );
			points[k].x = point.x;
			points[k].y = point.y;
			t = fpMath.m.add( t, deltaT );
		}
	}

	/**
	 * @return whether or not curve can be extrapolated (only for testing purposes, since bezier-curves
	 * can be extrapolated)
	 */
	public boolean CanBeExtrapolated() {
		return false;
	}

	/**
	 * Returns a Beziér curve representing the part of this curve in given domain [c,d].
	 * Let domain of this curve be [a,b]
	 * WARNING: d and a must not coincide! If that may happen use {@link Extrapolate()} instead.
	 * @see {@link ChangeDomain()} for more information
	 * @param fpMath
	 * @param newDomain
	 * @return
	 */
	public BezierCurve ChangeDomain( FPMath fpMath, Interval1d newDomain ) {
		// create resulting curve with same degree
		BezierCurve res = null;
		try {
			res = new BezierCurve( degree );
		}
		catch( Exception e ) {
			e.printStackTrace();
		}
		res.SetDomain( newDomain );

		// this array will take all intermediate points calculated
		Point2d[] points = new Point2d[controls.length];
		for( int i = 0; i < controls.length; i++ ) {
			points[i] = new Point2d( controls[i] );
		}

		// first of all generate curve for [a,d]
		Apfloat td = fpMath.m.divide( fpMath.m.subtract( newDomain.U(), domain.L() ), domain.Length() );
		DeCasteljauUpperDiag( fpMath, td, points, res.controls );

		// finally get part of [a,d]-curve representing [c,d]-part
		Apfloat tc = fpMath.m.divide( fpMath.m.subtract( newDomain.L(), domain.L() ),
																	fpMath.m.subtract( newDomain.U(), domain.L() ) );
		// restore starting points for Casteljau
		int len = res.controls.length;
		for( int i = 0; i < len; i++ ) {
			points[i].Set( res.controls[i] );
		}
		DeCasteljauLowerDiag( fpMath, tc, points, res.controls );
		return res;
	}

	/**
	 * Returns whether given curve c equals this curve.
	 * Curves are equal if they have the same control polygon
	 * @param c
	 * @return
	 */
	public boolean Equals( BezierCurve c ) {
		try {
			return Point2dSetOps.AreEqual( controls, c.controls );
		}
		catch( Exception e ) {
			return false;
		}
	}

	/**
	 * Extrapolates curve using De Casteljaus's algorithm.
	 * let domain of curve by [a,b]
	 * If c >= 1 curve is extrapolated to [b,a+(b-a)*c]
	 * If c <= 0 curve is extrapolated to [a+(b-a)*c,a]
	 * @param fpMath
	 * @param c
	 * @return
	 */
	public BezierCurve Extrapolate( FPMath fpMath, Apfloat c ) throws ArgumentException {
		if( c.compareTo( fpMath._0 ) > 0 || c.compareTo( fpMath._1 ) < 0 )
			throw new ArgumentException( "c MUST not be in (0.0,1.0)!" );
		BezierCurve resCurve = null;
		try {
			resCurve = new BezierCurve( degree );
		} catch( Exception e ) {
			e.printStackTrace();
		}

		if( c.compareTo( fpMath._1 ) > 0 )
			resCurve.domain = new Interval1d( domain.U(), fpMath.m.add( domain.L(), fpMath.m.multiply( c, domain.Length() ) ) );
		else
			resCurve.domain = new Interval1d( fpMath.m.add( domain.L(), fpMath.m.multiply( c, domain.Length() ) ), domain.L() );

		// temporary array for calculation of new control points
		Point2d[] points = new Point2d[controls.length];
		for( int i = 0; i < controls.length; i++ ) {
			points[i] = new Point2d( controls[i] );
		}

		if( c.compareTo( fpMath._0 ) >= 0 )
			DeCasteljauLowerDiag( fpMath, c, points, resCurve.controls );
		else
			DeCasteljauUpperDiag( fpMath, c, points, resCurve.controls );
		return resCurve;
	}

	/**
	 * Calculates hodograph (Derivative) or bezier-curve.
	 * If curve has degree zero, <code>null</code> is returned.
	 * @param fpMath
	 * @param scaleDown scaledown by degree?
	 * @return
	 */
	public BezierCurve Hodograph( FPMath fpMath, boolean scaleDown ) {
		if( degree == 0 )
			return null; // TODO return constant zero function here

		BezierCurve hodograph = null;
		try { hodograph = new BezierCurve( degree-1 ); } catch( Exception e ) {}
		hodograph.domain = this.domain;
		// simply calculate new control points
		Apfloat fdeg = scaleDown ? fpMath._1 : fpMath.ValueOf( (double)degree );
		for( int i = 0; i < degree; i++ ) {
			hodograph.controls[i].x = fpMath.m.multiply( fdeg, fpMath.m.subtract( controls[i+1].x, controls[i].x ) );
			hodograph.controls[i].y = fpMath.m.multiply( fdeg, fpMath.m.subtract( controls[i+1].y, controls[i].y ) );
		}
		return hodograph;
	}

	/**
	 * Returns whether curve is degenerated to one point
	 * @return
	 */
	public boolean IsDegenerated() {
		return (degree == 0 || Point2dSetOps.IsOnePoint( controls ));
	}

	public Tuple2<BezierCurve,BezierCurve> Subdivide( FPMath fpMath, Apfloat t0 ) throws ArgumentException {
		if( t0.compareTo( fpMath._0 ) < 0 || t0.compareTo( fpMath._1 ) > 0 )
			throw new ArgumentException( "Parameter \"t0\" must be in between 0.0 and 1.0!" );

		BezierCurve c0 = new BezierCurve( degree );
		c0.domain = new Interval1d( domain.L(), fpMath.m.add( domain.L(), fpMath.m.multiply( t0, domain.Length() ) ) );
		BezierCurve c1 = new BezierCurve( degree );
		Apfloat oneMinusT0 = fpMath._1.subtract( t0 );
		c1.domain = new Interval1d( fpMath.m.subtract( domain.U(), fpMath.m.multiply( oneMinusT0, domain.Length() ) ), domain.U() );

		// this array will take all intermediate points calculated
		Point2d[] points = new Point2d[controls.length];
		int i; // indices
		for( i = 0; i < controls.length; i++ ) {
			points[i] = new Point2d( controls[i] );
		}

		// first control point of first new curve is simply taken from old curve
		c0.controls[0].Set( points[0] );

		// last control point of second new curve is simply taken from old curve
		c1.controls[degree].Set( points[degree] );

		if( degree == 0 )
			return new Tuple2<BezierCurve, BezierCurve>( c0, c1 );

		int k; // indices

		// Calc DeCastlejau approximations
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				points[i].x = fpMath.m.multiplyAdd( oneMinusT0, points[i].x, t0, points[i+1].x );
				points[i].y = fpMath.m.multiplyAdd( oneMinusT0, points[i].y, t0, points[i+1].y );
			}

			// curve0 controls are points on diagonal P(i,0)
			// curve1 controls are points on diagonal P(i,deg-i)
			c0.controls[k].Set( points[0] );
			c1.controls[degree-k].Set( points[degree-k] );
		}
		return new Tuple2<BezierCurve, BezierCurve>( c0, c1 );
	}

	/**
	 * Sets ith control point to given coordinates (x,y)
	 * @param i index of control point (0..degree)
	 * @param x
	 * @param y
	 */
	public void SetControlPoint( int i, Apfloat x, Apfloat y ) {
		controls[i].x = x;
		controls[i].y = y;
	}

	/**
	 * Tries to calculate bounding wedge containing tangent vectors for given control points of a curve.
	 * It is assumed that controls contain at least two points and at least two points differ (original curve is not degnerated).
	 * The method used here not always finds the exact wedge!
	 * @param fpMath fixed-precision math
	 * @param controls control points
	 * @returns wedge or <code>null</code> if number of controls is lesser or equal one!
	 */
	public static BoundingWedge2d BoundingWedge( FPMath fpMath, Point2d[] controls ) {
		// no wedge for degree 0 because hodograph is zero!
		int deg = controls.length-1;
		if( deg <= 0 )
			return null;
		int i;
		// construct hodograph
		Point2d[] hodoVtcs = new Point2d[deg];
		Point2d temp;
		for( i = 0; i < deg; i++ ) {
			temp = new Point2d( fpMath.m.subtract( controls[i+1].x, controls[i].x ),
													fpMath.m.subtract( controls[i+1].y, controls[i].y ) );
			hodoVtcs[i] = temp;
		}
		// construct cone of vectors above and below x-axis
		Vector2d al = null, ar = null; // directions of left and right (clockwise) bounding vectors of above cone
		Vector2d bl = null, br = null; // directions of left and right (clockwise) bounding vectors of below cone

		// above
		// find vector above and below x-axis to start with
		LinkedList<Vector2d> above = new LinkedList<Vector2d>();
		LinkedList<Vector2d> below = new LinkedList<Vector2d>();
		for( i = 0; i < deg; i++ ) {
			if( hodoVtcs[i].y.compareTo( Apfloat.ZERO ) >= 0 ) {
				above.addLast( new Vector2d( hodoVtcs[i].x, hodoVtcs[i].y ) );
			}
			else {
				below.addLast( new Vector2d( hodoVtcs[i].x, hodoVtcs[i].y ) );
			}
		}

		Side s; // temporary

		if( above.size() > 0 ) {
			al = ar = above.removeFirst();
			for( Vector2d v : above ) {
				s = Vector2dOps.WhichSide( al, v );
				if( s == Side.Left ) {
					al = v;
					continue;
				}
				s = Vector2dOps.WhichSide( ar, v );
				if( s == Side.Right ) {
					ar = v;
				}
			}
		}

		// no vectors below x-axis -> we are done
		if( below.size() == 0 ) {
			return new BoundingWedge2d( new Vector2d( al ), Vector2dOps.Angle( fpMath, al, ar ) );
		}

		bl = br = below.removeFirst();
		for( Vector2d v : below ) {
			s = Vector2dOps.WhichSide( bl, v );
			if( s == Side.Left ) {
				bl = v;
				continue;
			}
			s = Vector2dOps.WhichSide( br, v );
			if( s == Side.Right ) {
				br = v;
			}
		}

		// no vectors found above x-axis -> we are done
		if( al == null ) {
			return new BoundingWedge2d( new Vector2d( bl ), Vector2dOps.Angle( fpMath, bl, br ) );
		}

		// both above and below contain vectors
		Apfloat angle_al = fpMath.m.subtract( fpMath.PI, ComplexOps.Argument( fpMath, al.x, al.y ) ); // (cw-) angle of al with negative x-axis
		Apfloat angle_ar = ComplexOps.Argument( fpMath, ar.x, ar.y ); // (ccw-) angle of ar with positive x-axis
		Apfloat angle_bl = fpMath.m.subtract( fpMath._2PI, ComplexOps.Argument( fpMath, bl.x, bl.y ) ); // (cw-) angle of bl with positive x-axis
		Apfloat angle_br = fpMath.m.subtract( ComplexOps.Argument( fpMath, br.x, br.y ), fpMath.PI ); // (ccw-) angle of br with negative x-axis
		// full cone?
		if( fpMath.m.add( angle_al, angle_br ).compareTo( fpMath.PI ) <= 0 &&
				fpMath.m.add( angle_ar, angle_bl ).compareTo( fpMath.PI ) <= 0 ) {
			return new BoundingWedge2d( new Vector2d( fpMath._1, fpMath._0 ), fpMath._2PI );
		}
		// cone opening to the negative x-axis
		else if( fpMath.m.add( angle_al, angle_br ).compareTo( fpMath.PI ) <= 0 ) {
			return new BoundingWedge2d( new Vector2d( bl ), fpMath.m.subtract( fpMath._2PI, fpMath.m.add( angle_ar, angle_bl ) ) );
		}
		// cone opening to the positive x-axis
		else {
			return new BoundingWedge2d( new Vector2d( al ), fpMath.m.subtract( fpMath._2PI, fpMath.m.add( angle_al, angle_br ) ) );
		}
	}

	/**
	 * Tries to calculate bounding wedge containing tangent vectors for this curve.
	 * Assumes that given curve is not degenerated.
	 * The method used here not always finds the exact wedge!
	 * @param fpMath fixed-precision math
	 * @param length length of wedge bounding vector
	 * @returns wedge or <code>null</code> if degree of curve is 0
	 */
	public BoundingWedge2d BoundingWedge( FPMath fpMath ) {
		// no bounding wedge for degree 0 because hodograph is zero!
		if( degree == 0 )
			return null;
		return BoundingWedge( fpMath, controls );
	}

	@Override
	public String toString() {
		return "["+"Degree: "+this.degree+", Domain: "+this.domain+"]";
	}
	//
	// AUXILLIARY METHODS
	//

	/**
	 * Calculates Bernstein polynom coefficients and puts result in coefficients-array
	 */
	private void CalculateCoefficients( FPMath fpMath ) {
		coeff[0] = fpMath._1;
		for( int i = 1; i <= degree; i++ ) {
			coeff[i] = fpMath.m.divide(
									fpMath.m.multiply( coeff[i-1], fpMath.ValueOf( (double)(degree-i+1) ) ),
									fpMath.ValueOf( (double)i )
								);
		}
	}

	/**
	 * Calculates the point on the bezier-curve at parameter "t" (in [0.0 .. 1.0]) directly by
	 * evaluating the bernstein polynomials. This method is numerically not very stable.
	 * @param fpMath
	 * @param t
	 * @param point resulting point
	 */
	private void CalculatePointBernstein( FPMath fpMath, Apfloat t, /*out*/ Point2d point ) {
		point.x = fpMath._0;
		point.y = fpMath._0;
		int i, j; // indices
		Apfloat polyval; // value of bernstein-polynomial

		// one time calculation of bernstein-polynomial-coefficents
		// if not already done
		if( coeff == null ) {
			coeff = new Apfloat[degree+1];
			CalculateCoefficients( fpMath );
		}

		for( i = 0; i <= degree; i++ ) {
			// calculate blending value for i
			polyval = coeff[i];
			for( j = 1; j <= i; j++ )
				polyval = fpMath.m.multiply( polyval, t );
			for( j = 1; j <= degree-i; j++ )
				polyval = fpMath.m.multiply( polyval, fpMath.m.subtract( fpMath._1, t ) );
			point.x = fpMath.m.add( point.x, fpMath.m.multiply( controls[i].x, polyval ) );
			point.y = fpMath.m.add( point.y, fpMath.m.multiply( controls[i].y, polyval ) );
		}
	}

	/**
	 * Calculates the point on the bezier-curve at parameter "t" (in [0.0 .. 1.0]) using
	 * De Casteljau's algorithm.
	 * This method is numerically more stable than calculating the Bernstein polynomial directly
	 * but its not as memory efficient.
	 * @param fpMath
	 * @param c
	 * @return
	 */
	private Point2d CalculatePointDeCasteljau( FPMath fpMath, Apfloat c ) {
		Point2d ret = new Point2d();
		CalculatePointDeCasteljau( fpMath, c, ret );
		return ret;
	}

	/**
	 * Calculates the point on the bezier-curve at parameter "t" (in [0.0 .. 1.0]) using
	 * De Casteljau's algorithm.
	 * This method is numerically more stable than calculating the Bernstein polynomial directly 
	 * but its not as memory efficient.
	 * @param fpMath
	 * @param c
	 * @param point
	 */
	private void CalculatePointDeCasteljau( FPMath fpMath, Apfloat c, /*out*/ Point2d point ) {
		int i, k; // indices
		Apfloat oneMinusC = fpMath.m.subtract( fpMath._1, c );
		// this array will take all intermediate points calculated.
		// this is what makes this method less memory-efficient
		Point2d[] points = new Point2d[controls.length];
		for( i = 0; i < controls.length; i++ ) {
			points[i] = new Point2d( controls[i] );
		}

		// Calc DeCastlejau approximations
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				points[i].x = fpMath.m.multiplyAdd( oneMinusC, points[i].x, c, points[i+1].x );
				points[i].y = fpMath.m.multiplyAdd( oneMinusC, points[i].y, c, points[i+1].y );
			}
		}

		point.x = points[0].x;
		point.y = points[0].y;
	}

	/**
	 * Helper method to calculate the intermediate points b(i,n-i) in the "lower" line
	 * of the De-Casteljau-triangle at parameter c.
	 * The resulting points are ordered: [b(n,0), b(n-1,1),...,#b(0,n)]
	 * Triangle expample:
	 * b(0,0)
	 *         b(1,0)
	 * b(0,1)         #b(2,0)
	 *         #b(1,1)
	 * #b(0,2)
	 * @param fpMath
	 * @param c
	 * @param intermPoints intermediate triangle points
	 * @param resDiagPoints resulting lower diagonal
	 */

	private void DeCasteljauLowerDiag( FPMath fpMath, Apfloat c, Point2d[] intermPoints, /*out*/ Point2d[] resDiagPoints ) {

		resDiagPoints[degree].Set( intermPoints[degree] );

		if( degree == 0 )
			return;

		int i, k; // indices
		Apfloat oneMinusC = fpMath.m.subtract( fpMath._1, c );

		// Calc DeCastlejau interm points
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				intermPoints[i].Set(
					fpMath.m.multiplyAdd( oneMinusC, intermPoints[i].x, c, intermPoints[i+1].x ),
					fpMath.m.multiplyAdd( oneMinusC, intermPoints[i].y, c, intermPoints[i+1].y )
				);
			}

			// controls are points on diagonal b(i,0)
			resDiagPoints[degree-k].Set( intermPoints[degree-k] );
		}
	}

	/**
	 * Helper method to calculate the intermediate points b(i,0) in the "upper" line
	 * of the De-Casteljau-triangle at local parameter c.
	 * The resulting points are ordered: [b(0,0), b(1,0),...,#b(n,0)]
	 * Triangle expample:
	 * #b(0,0)
	 *        #b(1,0)
	 * b(0,1)        #b(2,0)
	 *        b(1,1)
	 * b(0,2)
	 * @param fpMath
	 * @param c
	 * @param intermPoints intermediate triangle points
	 * @param resDiagPoints resulting upper diagonal
	 */
	private void DeCasteljauUpperDiag( FPMath fpMath, Apfloat c, Point2d[] intermPoints, /*out*/ Point2d[] resDiagPoints ) {

		resDiagPoints[0].Set( intermPoints[0] );

		if( degree == 0 )
			return;

		int i, k; // indices
		Apfloat oneMinusC = fpMath.m.subtract( fpMath._1, c );

		// Calc DeCastlejau interm points
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				intermPoints[i].Set(
					fpMath.m.multiplyAdd( oneMinusC, intermPoints[i].x, c, intermPoints[i+1].x ),
					fpMath.m.multiplyAdd( oneMinusC, intermPoints[i].y, c, intermPoints[i+1].y )
				);
			}

			// controls are points on diagonal b(i,0)
			resDiagPoints[k].Set( intermPoints[0] );
		}
	}

	//
	// MEMBERS
	//

	/**
	 * degree of curve
	 */
	private int degree;
	/**
	 * coefficients in bernstein polynomial (C(n,k))
	 */
	private Apfloat[] coeff;
	/**
	 * control points
	 */
	private Point2d[] controls;
	/**
	 * parameter domain
	 */
	Interval1d domain;
}
